# Copyright 2021 Josh Newans
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import math
import rclpy
from rclpy.node import Node

from std_msgs.msg import Header
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TwistWithCovariance
from geometry_msgs.msg import Pose
from geometry_msgs.msg import PoseWithCovariance
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TwistStamped
from geometry_msgs.msg import PoseStamped
from rclpy.qos import QoSProfile, ReliabilityPolicy
import numpy as np
# 在pose转换部分添加方向转换
from tf2_ros import TransformStamped, TransformBroadcaster
from geometry_msgs.msg import Quaternion
import tf2_ros
import tf_transformations   # ROS2中的tf2替代库
from message_filters import ApproximateTimeSynchronizer, Subscriber#订阅者同步处理

class TwistStamper(Node):

    def __init__(self):
        super().__init__('odom')#节点名称

        self.declare_parameter("child_link", "")# 声明一个名为'child_link'的参数，默认值为空字符串，参数可用于在运行时配置节点的某些特性
        self.child_link = str(self.get_parameter("child_link").value)# 获取刚刚声明的'child_link'参数的值，并转换为字符串类型，赋值给self.frame_id变量

        self.publisher_odom = self.create_publisher(Odometry, 'odom', 100)#创建发布方，发布odom
        # self.timer = self.create_timer(2.0,self.on_timer)

        # 设置为不可靠（BEST_EFFORT）的QoS策略
        qos_besteffort= QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT  # 设置为不可靠性
        )
        # 定义其他话题的QoS（RELIABLE）
        qos_reliable = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            depth=10
        )
        #创建订阅函数3个
        # self.ap_pose_sub = self.create_subscription(
        #     PoseStamped,
        #     'ap/pose/filtered',
        #     self.listener_callback_ap_pose_sub,
        #     qos_profile)
        # self.pose_cov_sub = self.create_subscription(
        #     PoseWithCovariance,
        #     'posewithcovariance',
        #     self.listener_callback_pose_cov_sub,
        #     10)
        # self.twist_cov_sub = self.create_subscription(
        #     TwistWithCovariance,
        #     'twistwithcovariance',
        #     self.listener_callback_twist_cov_sub,
        #     10)
        pose_withcov = Subscriber(self, PoseWithCovariance, 'posewithcovariance',qos_profile=qos_reliable)
        twist_withcov = Subscriber(self, TwistWithCovariance, 'twistwithcovariance',qos_profile=qos_reliable)
        header_appose = Subscriber(self, PoseStamped, 'ap/pose/filtered',qos_profile=qos_besteffort)

        # 配置时间同步器
        self.ts = ApproximateTimeSynchronizer(
            [pose_withcov, twist_withcov, header_appose],
            queue_size=35,#内部队列大小，决定缓存多少组消息以等待匹配。队列满时丢弃旧消息
            allow_headerless=True,
            slop=0.1#时间容差（秒），允许消息间的时间戳最大差异。例如，若三个消息的时间戳在0.1秒内，则视为同步。
        )
        self.ts.registerCallback(self.odom_publish)

        # self.subscription  # prevent unused variable warning
        # print("hello!pub_sub")

        # self.posewithcovariance = None
        # self.twistwithcovariance = None
        # self.posestampedheader = None

        # print("hello!None")
#四元数-------------------------------------------------------------------
        #创建一个 TransformBroadcaster 对象，用于向 ROS 2 的 TF 系统 广播坐标系之间的变换关系
        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
        #四元数orientation预先处理
        self.rotation_z = tf_transformations.quaternion_from_euler(0, 0, -math.pi/2)  # 绕Z轴-90°
        self.rotation_x = tf_transformations.quaternion_from_euler(math.pi, 0, 0)    # 绕X轴180°
        self.rotation_combined = tf_transformations.quaternion_multiply(
            self.rotation_x, self.rotation_z)  # 组合旋转self.rotation_combined 是 NED→ENU 的固定旋转，与 q_ned 相乘后，等效于将原始姿态旋转到 ENU 坐标系。
#-------------------------------------------
        self.get_logger().info("等待posewithcovariance and twistwithcovariance ...") 

    #修改以下
    def odom_publish(self):
        odom = Odometry()
        odom.child_frame_id = self.child_link
        odom.header = self.posestampedheader if self.posestampedheader else Header()

        # 坐标系转换：NED -> ENU
        if self.posewithcovariance is not None:
            # 获取NED坐标
            # ned_pose = self.posewithcovariance.pose.position

            # 坐标转换: 
            # NED的x->ENU的y, NED的y->ENU的x, NED的z->ENU的-z
            enu_pose = Pose()
            enu_pose.position.x = self.posewithcovariance.pose.position.y  # NED的东->ENU的东
            enu_pose.position.y = self.posewithcovariance.pose.position.x  # NED的北->ENU的北
            enu_pose.position.z = -self.posewithcovariance.pose.position.z  # NED的下->ENU的上

            odom.pose.pose = enu_pose  # 设置转换后的ENU坐标
            # odom.pose.covariance = self.posewithcovariance.pose.covariance
            #修改加上协方差矩阵-----------------------------------------
            # 协方差矩阵转换
            if len(self.posewithcovariance.covariance) == 36:
                cov_ned = np.array(self.posewithcovariance.covariance).reshape(6,6)#从消息中提取原始的6x6协方差矩阵，并将其从一维数组转换为二维矩阵（6行6列）。协方差矩阵按行优先顺序存储为36元素一维数组，需重塑为6x6以便操作。
                
                # 创建ENU协方差矩阵
                cov_enu = np.zeros((6,6))#创建一个全零的6x6矩阵，用于存储转换后的ENU协方差。
                
                # 位置协方差转换 (x,y,z <-> y,x,-z)
                cov_enu[0:3, 0:3] = cov_ned[[1,0,2], :][:, [1,0,2]] #将NED的x和y协方差交换到ENU的y和x，实现坐标轴转换。
                cov_enu[2,2] = cov_ned[2,2]  # z轴方差保持不变（符号变化不影响平方）
                
                # 姿态协方差保持原样（假设姿态数据已经是ENU）
                cov_enu[3:6, 3:6] = cov_ned[3:6, 3:6]#
                
                # 交叉协方差项处理
                cov_enu[0:3, 3:6] = cov_ned[[1,0,2], 3:6]
                cov_enu[3:6, 0:3] = cov_ned[3:6, [1,0,2]]
                
                odom.pose.covariance = cov_enu.flatten().tolist()
            else:
                self.get_logger().error("Invalid covariance matrix size!")
                return
            #orientation------------------------------------------
           # 姿态转换：应用预计算的旋转变换,从 ROS 消息中提取原始的四元数数据
            q_ned = [
                self.posewithcovariance.pose.orientation.x,
                self.posewithcovariance.pose.orientation.y,
                self.posewithcovariance.pose.orientation.z,
                self.posewithcovariance.pose.orientation.w
            ]
            #self.rotation_combined一个预定义的 坐标系转换四元数，表示从 NED 到 ENU 的旋转关系。
            #q_ned：原始姿态四元数（NED 坐标系）。
            #quaternion_multiply()：将两个四元数相乘，得到合成旋转。此处的作用是 将 NED 姿态转换到 ENU 坐标系。
            q_enu = tf_transformations.quaternion_multiply(self.rotation_combined, q_ned)
            #赋新值给odom
            odom.pose.pose.orientation.x = q_enu[0]
            odom.pose.pose.orientation.y = q_enu[1]
            odom.pose.pose.orientation.z = q_enu[2]
            odom.pose.pose.orientation.w = q_enu[3]
            #----------------------------------------orientation
        else:
            self.get_logger().warn("PoseWithCovariance is not set")
        # if self.twistwithcovariance is not None:
        #     odom.twist = self.twistwithcovariance  # 保持twist数据不变
        # else:
        #     self.get_logger().warn("TwistWithCovariance is not set")
            #对twist的坐标变换
        if self.twistwithcovariance is not None:
            # 转换线速度：NED的x(北) → ENU的y，NED的y(东) → ENU的x，z反向
            twist_enu = Twist()
            twist_enu.linear.x = self.twistwithcovariance.twist.linear.y  # 东向→ENU.x
            twist_enu.linear.y = self.twistwithcovariance.twist.linear.x  # 北向→ENU.y
            twist_enu.linear.z = -self.twistwithcovariance.twist.linear.z  # 下→上
    
            # 转换角速度：绕NED的Z轴旋转 → ENU的Z轴反向
            twist_enu.angular.x = self.twistwithcovariance.twist.angular.y
            twist_enu.angular.y = self.twistwithcovariance.twist.angular.x
            twist_enu.angular.z = -self.twistwithcovariance.twist.angular.z
            
            odom.twist.twist = twist_enu
        # 协方差转换（与pose类似）
            if len(self.twistwithcovariance.covariance) == 36:
                cov_ned = np.array(self.twistwithcovariance.covariance).reshape(6,6)
                cov_enu = np.zeros((6,6))
                # 线速度部分
                cov_enu[0:3, 0:3] = cov_ned[[1,0,2], :][:, [1,0,2]]
                cov_enu[2,2] = cov_ned[2,2]
                # 角速度部分
                cov_enu[3:6, 3:6] = cov_ned[[4,3,5], :][:, [4,3,5]]
                # 交叉项
                cov_enu[0:3, 3:6] = cov_ned[[1,0,2], 3:6]
                cov_enu[3:6, 0:3] = cov_ned[3:6, [1,0,2]]
                odom.twist.covariance = cov_enu.flatten().tolist()   
            else:
                self.get_logger().error("Invalid covariance matrix size!")
                return 
        else:
            self.get_logger().warn("TwistWithCovariance is not set") 
            #--------------------------------------------------------------------------------twist的修改
        # 发布转换后的odometry数据
        self.publisher_odom.publish(odom)
        self.get_logger().info(f'odom position 位置(ENU):\n' 
                               f' position_x:{odom.pose.pose.position.x}\n'
                               f' position_y: {odom.pose.pose.position.y}\n'
                               f' position_z: {odom.pose.pose.position.z}\n'
                               f'odom orientation朝向 (ENU):\n'
                               f' orientation(旋转轴在X向分量)_x: {odom.pose.pose.orientation.x}\n'
                               f' orientation()_y: {odom.pose.pose.orientation.y}\n'
                               f' orientation()_z: {odom.pose.pose.orientation.z}\n' 
                               f' orientation(旋转的“力度”)_w: {odom.pose.pose.orientation.w}\n' 
                               f' child_frame_id: {odom.child_frame_id}\n'
                               f' pose_covariance: {odom.pose.covariance}\n' 
                               f'odom angular角速度 (ENU):\n'
                               f' twist_angular_x: {odom.twist.twist.angular.x}\n'
                               f' twist_angular_y: {odom.twist.twist.angular.y}\n'
                               f' twist_angular_z: {odom.twist.twist.angular.z}\n'
                               f'odom linear线速度 (ENU):\n'
                               f' twist_linear_x: {odom.twist.twist.linear.x}\n'
                               f' twist_linear_y: {odom.twist.twist.linear.y}\n'
                               f' twist_linear_z: {odom.twist.twist.linear.z}\n'
                               f' twist_covariance: {odom.twist.covariance}\n')
    #-----------------------------------------------

def main(args=None):
    rclpy.init(args=args)

    twist_stamper = TwistStamper()
    rclpy.spin(twist_stamper)

    twist_stamper.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
